#include "Arduino.h"

#include <Ps3Controller.h>
#include <L298N.h>

const unsigned int IN1 = 33;
const unsigned int IN2 = 25;
const unsigned int PN_E1 = 18;

const unsigned int IN3 = 26;
const unsigned int IN4 = 27;
const unsigned int PN_E2 = 5;
const unsigned int IN5 = 12;
const unsigned int IN6 = 14;
const unsigned int PN_E3 = 17;

const unsigned int IN7 = 2;
const unsigned int IN8 = 15;
const unsigned int PN_E4 = 16;

// Create one motor instance    
L298N motor(PN_E1,IN1, IN2);
L298N motor2(PN_E2,IN3, IN4);
L298N motor3(PN_E3,IN5, IN6);
L298N motor4(PN_E4,IN7, IN8);

int isPress = 0;
void setSpeed(){
    motor.setSpeed(50);
    motor2.setSpeed(50);
    motor3.setSpeed(50);
    motor4.setSpeed(50);
}
void notify()
{
    isPress = 1;
    if( Ps3.data.button.cross ){
        Serial.println("Pressing the cross button");
    }

    if( Ps3.data.button.square ){
        Serial.println("Pressing the square button");
    }

    if( Ps3.data.button.triangle ){
        Serial.println("Pressing the triangle button");
    }

    if( Ps3.data.button.circle ){
        Serial.println("Pressing the circle button");
    }
    Serial.print("Analog lx:");
    Serial.print(Ps3.data.analog.stick.lx);
    Serial.print("ly:");
    Serial.println(Ps3.data.analog.stick.ly);

    //向上
    if (Ps3.data.analog.stick.ly < -30 and (Ps3.data.analog.stick.lx < 30 and Ps3.data.analog.stick.lx > -30)) {
            //将IN1设置为高电频，IN2设置为低电频，电机正转
//        digitalWrite(IN1,HIGH);
//        digitalWrite(IN2,LOW);
//
//        digitalWrite(IN3,HIGH);
//        digitalWrite(IN4,LOW);

        setSpeed();

        motor.forward();
        motor2.forward();
        motor3.forward();
        motor4.forward();
        Serial.println("forward");
    }
    //向下
    else if (Ps3.data.analog.stick.ly > 30 and (Ps3.data.analog.stick.lx < 30 and Ps3.data.analog.stick.lx > -30)) {
        //将IN1设置为低电频，IN2设置为高电频，电机反转
        setSpeed();
        motor.backward();
        motor2.backward();
        motor3.backward();
        motor4.backward();
        Serial.println("backward");
    }
    //向左
    else if (Ps3.data.analog.stick.lx < -30 and  (Ps3.data.analog.stick.ly > -30 and Ps3.data.analog.stick.ly < 30)) {
        //将IN1设置为高电频，IN2设置为低电频，电机正转
        setSpeed();
        motor.backward();
        motor2.backward();
        motor3.forward();
        motor4.forward();

        Serial.println("left");
    }
    //向右
    else if (Ps3.data.analog.stick.lx > 30 and (Ps3.data.analog.stick.ly > -30 and Ps3.data.analog.stick.ly < 30) ) {
        //将IN1设置为低电频，IN2设置为高电频，电机反转
        setSpeed();
        motor.forward();
        motor2.forward();
        motor3.backward();
        motor4.backward();

        Serial.println("right");
    }//向右上
    else if (Ps3.data.analog.stick.lx > 30  and Ps3.data.analog.stick.ly < -30) {
        //将IN1设置为低电频，IN2设置为高电频，电机反转
        motor.forward();
        motor2.forward();
        motor.setSpeed(30);
        motor2.setSpeed(30);

        motor3.forward();
        motor4.forward();

        Serial.println("right up");
    }//向左上
    else if (Ps3.data.analog.stick.lx < -30  and Ps3.data.analog.stick.ly < -30) {
        //将IN1设置为低电频，IN2设置为高电频，电机反转
        motor3.setSpeed(30);
        motor4.setSpeed(30);

        motor.forward();
        motor2.forward();
        motor3.forward();
        motor4.forward();

        Serial.println("left up");
    }
    else{

        motor.stop();
        motor2.stop();
        motor3.stop();
        motor4.stop();
    }
////    motor.stop();
////    motor2.stop();
}

void onConnect(){
    Serial.println("Connected!.");
}

void setup()
{
    Serial.begin(115200);
    Ps3.attach(notify);
    Ps3.attachOnConnect(onConnect);
//    Ps3.begin("a0:b7:65:f5:36:4e");
    //esp32 Address e4:65:b8:d8:e7:82
    Ps3.begin("8c:e9:ee:1e:94:65");
    Serial.println("Ready.");

    //设置pin 输出模式
    pinMode(IN1,OUTPUT);
    pinMode(IN2,OUTPUT);
//
    pinMode(IN3,OUTPUT);
    pinMode(IN4,OUTPUT);

    pinMode(IN5,OUTPUT);
    pinMode(IN6,OUTPUT);

    pinMode(IN7,OUTPUT);
    pinMode(IN8,OUTPUT);

    motor.setSpeed(50);
    motor2.setSpeed(50);
    motor3.setSpeed(50);
    motor4.setSpeed(50);

//    Serial.begin(115200);
//    Ps3.begin();
//
//    String address = Ps3.getAddress();
//
//    Serial.print("The ESP32's Bluetooth MAC address is: ");
//    Serial.println(address);
}

void loop()
{
    if(isPress == 1){
        isPress = 0;
        delay(1000);
    }
    else{
        motor.stop();
        motor2.stop();
        motor3.stop();
        motor4.stop();
    }
}
